# -*- coding: utf-8 -*-

'''

根据实际测试情况，准备一些测试数据往下发 

下发速度前先让 角度 调整到位 

'''

import time 
import binascii
import math
import struct
import ast

from s3_1 import gen_vw_list


real_send = 1       # 是否真的写串口
usb_serial = 'COM1' # 串口号  


class Foo:
    # 这个类是用来测试的，如果不写串口，就调用这个类的方法
    def write(self, arg):
        data= str(binascii.b2a_hex( arg ))[2:-1]          #Hex转换成字符串
        # print(data)

    def inWaiting(self):
        return 1
    
    def read(self, n):
        return '1'


# 这里初始化了串口
if real_send:
    # pip uninstall serial
    # pip install pyserial 
    
    import serial
    ser = serial.Serial(usb_serial, 115200, timeout=0.5)  #    115200  256000
else:
    ser = Foo()




################
# crc 
GENERATOR_POLYNOMIAL = 0x8005   # CRC16生成多项式：X16+X15+X2+1 
CRC = 0xFFFF   # CRC初始值为：0xFFFF 

def calculateonebyte(databyte, tempcrc): 
    """ 
    计算1字节数据的CRC值 
    :param databyte: 需计算的字节数据 
    :param tempcrc: 当前的CRC值 
    :return: 得到新的CRC值 
    """ 
    # databyte必须为字节数据 
    # assert 0x00 <= databyte <= 0xFF 
    # 同上字节数据检查 
    if not 0x00 <= databyte <= 0xFF: 
        raise Exception((u'数据：0x{0:<02X}不是字节数据[0x00-0xFF]'.format(databyte)).encode('utf-8')) 
 
    # 把字节数据根CRC当前值的低8位相异或 
    low_byte = (databyte ^ tempcrc) & 0x00FF 
    # 当前CRC的高8位值不变 
    resultCRC = (tempcrc & 0xFF00) | low_byte 
 
    # 循环计算8位数据 
    for index in range(8): 
        # 若最低为1：CRC当前值跟生成多项式异或;为0继续 
        if resultCRC & 0x0001 == 1: 
            #print("[%d]: 0x%4X ^^^^ 0x%4X" % (index,resultCRC>>1,resultCRC^GENERATOR_POLYNOMIAL)) 
            resultCRC >>= 1 
            resultCRC ^= 0xA001 # 0xA001是0x8005循环右移16位的值 
        else: 
            # print ("[{0}]: 0x{1:X} >>>> 0x{2:X}".format(index,resultCRC,resultCRC>>1)) 
            resultCRC >>= 1 
 
    return resultCRC 

def calculateCRC(data): 
    """ 
    检查输入数据是否合法 
    :param dataarray: 需要生成CRC校验的数据 
    :return: 合法的数据元组 

    data = '48505a415555555555555555aa2600640000ff9c00000000400000000000014807d0f830'  # '0db4'

    """ 

    assert len(data)%2 == 0, 'calculateCRC: input data len must be 2*n '

    datalist = []
    for i in range(  int(0.5*len(data))  ):
        # print('dd:', data[ i*2 :i*2+2 ] )
        datalist.append( '0x'+data[ i*2 :i*2+2 ]  )

    index = 0 
    try: 
        # 将输入的字符串按不同进制数转化为数据序列 
        for index, item in enumerate(datalist): 
            datalist[index] = int(item, 16) 

        # print (u'成功转换后的对应的序列：\n {0}'.format(datalist)) 

        # 处理第1个字节数据 
        temp = calculateonebyte(datalist.pop(0), 0xFFFF) 
        # 循环处理其它字节数据 
        for data in datalist: 
            temp = calculateonebyte(data,temp) 
        # print('type temp:', type(temp) ) # ('type temp:', <type 'int'>)
        
        crc = struct.pack('<I', temp).hex()

        rst = crc[0:4]

        return rst
 
    except ValueError as err: 
        print(u'第{0}个数据{1}输入有误'.format(index,datalist[index]).encode('utf-8')) 
        print(err) 
    # finally: 
    #     print('当前datalist:{0} '.format(datalist)) 


################

def hex_str_to_bytes(hex_str):
    int_list = []
    ll = int(0.5*len(hex_str))
    for i in range( ll ):
        int_list.append( int(hex_str[i:i+2], 16) )
    
    bytes_buff = bytearray( int_list )
    # print('buff:', bytes_buff )
    return bytes_buff



def write_ser(hex_str):

    print('write_ser hex_str:', hex_str )
    # print('type hex:', type(hex_str), len(hex_str) )  # <type 'str'>  59
    
    buff = bytes.fromhex(hex_str)
    print('buff:', buff)


    ser.write(buff)
    # time.sleep(0.001)
 

##########################

def gen_twist_buf(v1, v2, v3, v4, deg1, deg2, deg3, deg4):
    # buf = gen_twist_buf(v1, v2, v3, v4, deg1, deg2, deg3, deg4)

    head = 'fffe'
    # buf_2 = 3 # type_twist  1字节
    buf_2 = 14 # type_twist  1字节

    buf_type = struct.pack('<B', buf_2).hex()

    # print('buf_type:', buf_type) # buf_type: 03

    buf_3 = 44 # buf_len  2 字节  H   华能速度下发 会 下发 44 字节给底盘
    buf_len = struct.pack('<H', buf_3).hex()

    # print('buf_len:', buf_len) # buf_len: 2c00

    buf_5 = 255 - (buf_3 + 0) % 256
    buf_len_crc = struct.pack('<B', buf_5).hex()


    # print('buf_len_crc:', buf_len_crc)  # buf_len_crc: d3

    ts = 1 # 时间戳
    buf_ts = struct.pack('<I', ts).hex()

    # print('buf_ts:', buf_ts)  # buf_ts: 01000000

    buf_v1 = struct.pack('<f', v1).hex()
    buf_v2 = struct.pack('<f', v2).hex()
    buf_v3 = struct.pack('<f', v3).hex()
    buf_v4 = struct.pack('<f', v4).hex()

    buf_deg1 = struct.pack('<f', deg1).hex()
    buf_deg2 = struct.pack('<f', deg2).hex()
    buf_deg3 = struct.pack('<f', deg3).hex()
    buf_deg4 = struct.pack('<f', deg4).hex()


    rst = head + buf_type + buf_len + buf_len_crc + buf_ts + buf_v1 + buf_v2 + buf_v3 + buf_v4 + \
        buf_deg1 + buf_deg2 + buf_deg3 + buf_deg4

    crc_rst = calculateCRC(rst) 

    crc_rst_reverse = crc_rst[2:4]+crc_rst[0:2]
    # print('crc_rst:', crc_rst_reverse ) # 98ed

    rst = rst + crc_rst_reverse
    
    print('buf:\n', rst)
    # fffe032c00d3cdcccc3dcdcccc3dcdcccc3dcdcccc3d0000204100002041000020410000204198ed

    buff = bytes.fromhex(rst)
    # print('buff3:', buff)


    return buff

if __name__ == '__main__':

    ###  四个轮子的线速度 和 朝向角 
    
    mode = 4

    # 1, w  原地转弯模式
        # deg  -52.812, 52.812, 52.812, -52.812
        # 左转    <0       >0     <0       >0 
        # 右转    >0       <0     >0       <0  

    if mode == 1:
        vl = [-5, 5, -5, 5] # 左转
        # vl = [ 5,-5,  5,-5] # 右转

        dl = [-52.812, 52.812, 52.812, -52.812]

    # 2, v, w 正常运动模式
    if mode == 2:
        v = -0.18 
        w = -0.15

        vl, dl = gen_vw_list(v, w) 

        vl = [e*50 for e in vl]

        print('v_list:', vl)
        print('theta_list:', dl)
    # 3, r 调整轮子朝向模式
    if mode == 3:
        r = 1.0
        w = -0.1
        v = w*r 
        vl, dl = gen_vw_list(v, w)
        vl = [0, 0, 0, 0]

    # 4, v, theta 斜行模式
        # deg 在正负30度内即可，速度 同正同负 
    
    if mode == 4:
        vv = -5
        deg = 0

        vl = [vv, vv, vv, vv]
        dl = [deg, deg, deg, deg]


    # 5, v, theta = 90  横行模式
        # deg  -90, 90, 90, -90 
        # 向左  <0   >0  >0  <0 
        # 向右  >0   <0  <0  >0  

    if mode == 5:
        dl = [-90, 90, 89, -88]
        # vl = [-5, 5, 5, -5]   # 向左
        vl = [ 5,-5,-5,  5] # 向右 

    # vel_list   vl 
    # deg_list   dl

    print('v = 0')
    buf_v_0 = gen_twist_buf(    0,     0,     0,     0, dl[0], dl[1], dl[2], dl[3])
    print('v normal')
    buf =     gen_twist_buf(vl[0], vl[1], vl[2], vl[3], dl[0], dl[1], dl[2], dl[3])

    # print(len(buf))

    t1 = time.time() 

    while 1 and real_send: # 如果是真的接在串口上, 就往下发
        
        t2 = time.time() 

        if t2-t1 <= 3:
            buf_to_send = buf_v_0 
        else:
            buf_to_send = buf 

        if t2-t1 >= 3.0+4.0:
            break 

        ser.write(buf_to_send)

        time.sleep(0.05) # 写串口的频率 20 hz 
